#include "EvadeState.h"
#include "PDController.h"
#include "KalmanState.h"

EvadeState::EvadeState(){}

EvadeState::EvadeState(KalmanTank* enemy, int direction) {
	this->enemy = enemy;
	this->direction = direction;
}


EvadeState::~EvadeState(void) {
}

Command EvadeState::GetCommand(Agent* agent){
	agent->setState(new KalmanState(enemy));
	return agent->GetCommand();
	
	/*Vector2 tankPos(agent->getTank()->pos[0], agent->getTank()->pos[1]);
	if(InDanger(tankPos)) {
		return PDController::instance()->GetCommand(agent->getTank(), CalcEvadeTrajectory(tankPos));
	}
	return Command(agent->getTank()->index, 0, 0, false);*/
}

Vector2 EvadeState::CalcEvadeTrajectory(Vector2 pos){
	Vector2 dir = pos - enemy->GetCurrentEstimate();
	
	return Vector2(dir.y, -dir.x) * direction;
}

bool EvadeState::InDanger(Vector2 pos){
	float dir = (pos - enemy->GetCurrentEstimate()).angle();
	float enemyDir = enemy->GetStruct()->angle;

	dir = NormalizeAngle(dir);
	enemyDir = NormalizeAngle(enemyDir);

	float diff = fabs(dir - enemyDir);
	
	if(diff > M_PI)
		diff = 2 * M_PI - diff;
	
	if(diff < DANGER_THRESHOLD)
		return true;
	return false;
}

float EvadeState::NormalizeAngle(float angle){
	if (angle > 0) {
		while (angle > 2 * M_PI) {
			angle -= 2 * M_PI;
		}
	} else {
		while (angle < 0) {
			angle += 2 * M_PI;
		}
	}

	return angle;
}